#include "DFRobot_K10Box.h"
#include <string>
#include <bitset>
#include <Arduino.h>


DFRobot_K10Box::DFRobot_K10Box()
{

}

int16_t DFRobot_K10Box::begin(void)
{
  Wire.begin(47,48);
  return 0;
}


uint32_t DFRobot_K10Box::rgbToColor(uint8_t r, uint8_t g, uint8_t b)
{
  return (uint32_t)((((uint32_t)r<<16) | ((uint32_t)g<<8)) | (uint32_t)b);
}


//获取声音大小值
uint16_t DFRobot_K10Box::getSoundVolume(void){
  uint16_t value;
  uint8_t tempbuf[2];
  readReg(I2C_MIC_DATA0, tempbuf, 2);
  value = tempbuf[0]<<8 | tempbuf[1];
  return value;
}

//获取旋钮大小值
uint16_t DFRobot_K10Box::getKnobValue(){
  uint16_t value;
  uint8_t tempbuf[2];
  readReg(I2C_VR_DATA0, &tempbuf, 2);
  value = tempbuf[0]<<8 | tempbuf[1];
  return value;
}

//获取红外接收码
uint32_t DFRobot_K10Box::getIRValue(){
  uint8_t buf[5];
  uint32_t ir = 0;
  readReg(I2C_IR_STATE, buf, 5);
  if(buf[0] == IR_DATA_ENABLE){
    ir = ((uint32_t)buf[1])<<24 | ((uint32_t)buf[2])<<16 |((uint32_t)buf[3])<<8 | buf[4];
  }
  return ir;
}

//获取SR04的距离值
uint16_t DFRobot_K10Box::getSR04Value(){
  uint8_t sr04Status = SR04_STATE_COLLECT;
  uint8_t tempBuf[3];
  uint16_t distance = 0xFFFF;
  writeReg(I2C_SR04_STATE, &sr04Status, 1);
  delay(100);
  readReg(I2C_SR04_STATE, tempBuf ,3);
  if(tempBuf[0] == SR04_STATE_COMPLETE){
    distance = ((uint16_t)tempBuf[1])<<8 | tempBuf[2];
    if(distance < SR04_COLLECT_TIMEROUT){
    }else{
      distance = 0xFFFF;
    }
  }
  return distance;
}

//设置两个电机占空比，A和B的值为0~255
void DFRobot_K10Box::setMotor1(uint8_t M1A, uint8_t M1B){
  //1Khz
  uint16_t freq = 1000000/1000;
  uint16_t m1aVal,m1bVal;
  uint8_t tempbuf[4];
  tempbuf[0] = freq >> 8 & 0xFF;
  tempbuf[1] = freq  & 0xFF;
  writeReg(I2C_PWM1_FREQ0, tempbuf, 2);
  m1aVal = map(M1A, 0, 255, 0, 1000);
  m1bVal = map(M1B, 0, 255, 0, 1000);
  tempbuf[0] = m1aVal>>8;
  tempbuf[1] = m1aVal;
  tempbuf[2] = m1bVal>>8;
  tempbuf[3] = m1bVal;
  writeReg(I2C_PWM1_DUTY0, tempbuf, 4);
}

//设置电机转动
void DFRobot_K10Box::setMotor1Run(uint8_t dir, uint8_t speed){
  uint8_t m1a,m1b;
  if(dir == 0){//正转
    m1a = speed;
    m1b = 0;
  }else{
    m1a = 0;
    m1b = speed;
  } 
  setMotor1(m1a,m1b);
}

void DFRobot_K10Box::setMotor2(uint8_t M2A, uint8_t M2B){
  //1KhZ
  uint16_t freq = 1000000/1000;
  uint16_t m2aVal,m2bVal;
  uint8_t tempbuf[4];
  tempbuf[0] = freq >> 8 & 0xFF;
  tempbuf[1] = freq  & 0xFF;
  writeReg(I2C_PWM1_FREQ0, tempbuf, 2);
  m2aVal = map(M2A, 0, 255, 0, 1000);
  m2bVal = map(M2B, 0, 255, 0, 1000);
  tempbuf[0] = m2aVal>>8;
  tempbuf[1] = m2aVal;
  tempbuf[2] = m2bVal>>8;
  tempbuf[3] = m2bVal;
  writeReg(I2C_PWM1_DUTY4, tempbuf, 4);
}


// M2电机方向反了，已修正
void DFRobot_K10Box::setMotor2Run(uint8_t dir, uint8_t speed){
  uint8_t m2a,m2b;
  if(dir == 0){//正转
    m2a = 0;
    m2b = speed;
  }else{
    m2a = speed;
    m2b = 0;
  } 
  setMotor2(m2a,m2b);
}



//设置蜂鸣器的频率，单位为hz，范围为1M/(1~65535)=1M~15(HZ)
void DFRobot_K10Box::setBuzzer(uint16_t freq){
  if(freq == 0){
    stopBuzzer();
    return;
  }
  uint32_t realfreq = 1000000/freq;
  uint32_t realduty = realfreq/2;
  uint8_t tempbuf[4];
  tempbuf[0] = realfreq >> 8 & 0xFF;
  tempbuf[1] = realfreq & 0xFF;
  tempbuf[2] = realduty >> 8 & 0xFF;
  tempbuf[3] = realduty & 0xFF;
  writeReg(I2C_PWM3_FREQ0, tempbuf, 4);
}


void DFRobot_K10Box::stopBuzzer(){
  uint16_t temp = 0x00;
  writeReg(I2C_PWM3_DUTY2, &temp, 2);
}


//设置LED灯亮度
void DFRobot_K10Box::red_analog(uint8_t R){
  uint16_t freq = 255;
  uint8_t tempbuf[2];
  tempbuf[0] = 0x00;
  tempbuf[1] = freq;
  writeReg(I2C_PWM2_FREQ0, tempbuf, 2);
  tempbuf[0] = 0x00;
  tempbuf[1] = R;
  writeReg(I2C_PWM2_DUTY4, tempbuf, 2);
}

void DFRobot_K10Box::yellow_analog(uint8_t Y){
  uint16_t freq = 255;
  uint8_t tempbuf[2];
  tempbuf[0] = 0x00;
  tempbuf[1] = freq;
  writeReg(I2C_PWM2_FREQ0, tempbuf, 2);
  tempbuf[0] = 0x00;
  tempbuf[1] = Y;
  writeReg(I2C_PWM2_DUTY2, tempbuf, 2);
}

void DFRobot_K10Box::green_analog(uint8_t G){
  uint16_t freq = 255;
  uint8_t tempbuf[2];
  tempbuf[0] = 0x00;
  tempbuf[1] = freq;
  writeReg(I2C_PWM2_FREQ0, tempbuf, 2);
  tempbuf[0] = 0x00;
  tempbuf[1] = G;
  writeReg(I2C_PWM2_DUTY0, tempbuf, 2);
}

// 通过0、1控制灯亮灭

void DFRobot_K10Box::red_digital(uint8_t state){
  uint8_t R = state ? 255 : 0;  
  uint16_t freq = 255;
  uint8_t tempbuf[2];
  tempbuf[0] = 0x00;
  tempbuf[1] = freq;
  writeReg(I2C_PWM2_FREQ0, tempbuf, 2);
  tempbuf[0] = 0x00;
  tempbuf[1] = R;
  writeReg(I2C_PWM2_DUTY4, tempbuf, 2);
}

void DFRobot_K10Box::yellow_digital(uint8_t state){
  uint8_t Y = state ? 255 : 0;  
  uint16_t freq = 255;
  uint8_t tempbuf[2];
  tempbuf[0] = 0x00;
  tempbuf[1] = freq;
  writeReg(I2C_PWM2_FREQ0, tempbuf, 2);
  tempbuf[0] = 0x00;
  tempbuf[1] = Y;
  writeReg(I2C_PWM2_DUTY2, tempbuf, 2);
}

void DFRobot_K10Box::green_digital(uint8_t state){
  uint8_t G = state ? 255 : 0;  
  uint16_t freq = 255;
  uint8_t tempbuf[2];
  tempbuf[0] = 0x00;
  tempbuf[1] = freq;
  writeReg(I2C_PWM2_FREQ0, tempbuf, 2);
  tempbuf[0] = 0x00;
  tempbuf[1] = G;
  writeReg(I2C_PWM2_DUTY0, tempbuf, 2);
}



// //巡线模块
// void DFRobot_K10Box::initLinePatrol(void){
//   Wire.begin(47,48);
// }

//设置巡线指定探头的阈值
void DFRobot_K10Box::setLinePatrolThreshold(uint8_t num, uint8_t threshold){
  if(num < 5){
    writeReg(LINE_PATROL_ADDR, num + 0x0E, &threshold, 1);
    delay(100);
  }
}

//获取巡线指定探头的阈值
uint8_t DFRobot_K10Box::getLinePatrolThreshold(uint8_t num){
  uint8_t threshold;
  if(num < 5){
    readReg(LINE_PATROL_ADDR, num + 0x0E, &threshold, 1);
  }
  return threshold;
}

// uint8_t* DFRobot_K10Box::LinePatrolThresholdAll(){
//   uint8_t thresholdAll[5];
//   readReg(LINE_PATROL_ADDR, 0x0E, thresholdAll, 5);
//   return thresholdAll;
// }

String DFRobot_K10Box::getLinePatrolThresholdAll() {
  uint8_t thresholdAll[5];
  readReg(LINE_PATROL_ADDR, 0x0E, thresholdAll, 5);
  String thresholdString = "";
  for (int i = 0; i < 5; i++) {
    thresholdString += String(thresholdAll[i], DEC); 
    if (i < 4) { 
      thresholdString += ", ";
    }
  }
  return thresholdString;  
}

uint8_t DFRobot_K10Box::getLinePatrolValue(uint8_t num){
  uint8_t value;
  uint8_t threshold;

  if(num < 5){
    readReg(LINE_PATROL_ADDR, num + 0x09, &value, 1);
    readReg(LINE_PATROL_ADDR, num + 0x0E, &threshold, 1);
    if (value > threshold){
      return 0;
    }else{
      return 1;
    }
  }
  // return value;
}

// //获取所有巡线探头的原始值
// uint8_t* DFRobot_K10Box::getLinePatrolValueAll(){
//   uint8_t valueAll[5];
//   readReg(LINE_PATROL_ADDR, 0x09, valueAll, 5);
//   return valueAll;
// }

//获取所有巡线探头的原始值
String DFRobot_K10Box::getLinePatrolValueAll(){
  uint8_t valueAll[5];
  readReg(LINE_PATROL_ADDR, 0x09, valueAll, 5);
  String valueString = "";
  for (int i = 0; i < 5; i++) {
    valueString += String(valueAll[i], DEC); 
    if (i < 4) { 
      valueString += ", ";
    }
  }
  return valueString;
}


String DFRobot_K10Box::getLinePatrolStatusAll(){
  uint8_t status;
  readReg(LINE_PATROL_ADDR, 0x08, &status, 1);

  String binaryString = "";
  for (int i = 0; i < 5; i++) {
    binaryString += ((status >> i) & 0x01) ? '1' : '0';
  }
  return binaryString;
}


// 加速度配置

uint8_t DFRobot_K10Box::readQMI8658CID(){
  uint8_t ret,regValue;
  readReg(QMI8658C_ADDR, 0x00, &ret, 1);
  if(ret == 0x05){
    regValue = 0x60 | 0x10 | 0x08;
    writeReg(QMI8658C_ADDR, 0x02, &regValue, 1);
    regValue = 0x00;
    writeReg(QMI8658C_ADDR, 0x08, &regValue, 1);
    regValue = 0xC0;
    writeReg(QMI8658C_ADDR, 0x09, &regValue, 1);
  }
  return ret;
}

void DFRobot_K10Box::setQMI8658CMode(uint8_t mode){
  writeReg(QMI8658C_ADDR, 0x08, &mode, 1);
}

void DFRobot_K10Box::setQMI8658CAccConfig(uint8_t rang, uint8_t odr){
  uint8_t regValue,temp[2];
  switch(rang){
    case QMI8658_ACCRANGE_2G:
      this->ssvtA = 1 << 14;
      break;
    case QMI8658_ACCRANGE_4G:
      this->ssvtA = 1 << 13;
      break;
    case QMI8658_ACCRANGE_8G:
      this->ssvtA = 1 << 12;
      break;
    case QMI8658_ACCRANGE_16G:
      this->ssvtA = 1 << 11;
      break;
    default:
      rang = QMI8658_ACCRANGE_8G;
      this->ssvtA = 1 << 12;
      break;
  }
  regValue = rang | odr;
  writeReg(QMI8658C_ADDR, 0x03, &regValue, 1);
  readReg(QMI8658C_ADDR,0x06,temp,1);
  regValue = temp[0] & 0xF0;
  regValue &= ~0x01;
  writeReg(QMI8658C_ADDR, 0x06, &regValue, 1);
}

void DFRobot_K10Box::setQMI8658CGyroConfig(uint8_t rang, uint8_t odr){
  uint8_t temp[2],regValue;
  switch(rang){
    case QMI8658_GRYORANG_16DPS:
      this->ssvtG = 2048;
      break;
    case QMI8658_GRYORANG_32DPS:
      this->ssvtG = 1024;
      break;
    case QMI8658_GRYORANG_64DPS:
      this->ssvtG = 512;
      break;
    case QMI8658_GRYORANG_128DPS:
      this->ssvtG = 256;
      break;
    case QMI8658_GRYORANG_256DPS:
      this->ssvtG = 128;
      break;
    case QMI8658_GRYORANG_512DPS:
      this->ssvtG = 64;
      break;
    case QMI8658_GRYORANG_1024DPS:
      this->ssvtG = 32;
      break;
    case QMI8658_GRYORANG_2048DPS:
      this->ssvtG = 16;
      break;
    default:
      rang = QMI8658_GRYORANG_512DPS;
      this->ssvtG = 64;
      break;
  }
  regValue = rang | odr;
  writeReg(QMI8658C_ADDR, 0x04, &regValue, 1);
  readReg(QMI8658C_ADDR, 0x06, temp, 1);
  regValue = temp[0] & 0x0F;
  regValue &= ~0x10;
  writeReg(QMI8658C_ADDR, 0x06, &regValue, 1);
}


void DFRobot_K10Box::initQMI8658C(){
  uint8_t temp;
  Wire.begin(47,48);
  //QMI rst
  //temp = 0xB0;
  //writeReg(QMI8658C_ADDR,0x60, &temp, 1);
  //delay(10);
  //配置模式为ACC和GYRO
  readQMI8658CID();
  setQMI8658CMode(0x00);
  setQMI8658CAccConfig(QMI8658_ACCRANGE_8G, QMI8658_ACCODR_250HZ);
  setQMI8658CGyroConfig(QMI8658_GRYORANG_1024DPS, QMI8658_GRYOODR_250HZ);
  setQMI8658CMode(0x03); 
}

float DFRobot_K10Box::getAccelerometerXms2(){
  getQMI8658xyz();
  return (float)(this->accX * 9.807)/this->ssvtA;
}

float DFRobot_K10Box::getAccelerometerYms2(){
  getQMI8658xyz();
  return (float)(this->accY * 9.807)/this->ssvtA;
}

float DFRobot_K10Box::getAccelerometerZms2(){
  getQMI8658xyz();
  return (float)(this->accZ * 9.807)/this->ssvtA;
}

// 加了获取加速度强度
float DFRobot_K10Box::getStrengthms2(){
  getQMI8658xyz();
  float x = getAccelerometerXms2();
  float y = getAccelerometerYms2();
  float z = getAccelerometerZms2();
  return sqrt(x * x + y * y + z * z);
}

float DFRobot_K10Box::getAccelerometerXmg(){
  getQMI8658xyz();
  return (float)(this->accX * 1000.0)/this->ssvtA;
}

float DFRobot_K10Box::getAccelerometerYmg(){
  getQMI8658xyz();
  return (float)(this->accY * 1000.0)/this->ssvtA;
}

float DFRobot_K10Box::getAccelerometerZmg(){
  getQMI8658xyz();
  return (float)(this->accZ * 1000.0)/this->ssvtA;
}

// 加了获取加速度强度
float DFRobot_K10Box::getStrengthmg(){
  getQMI8658xyz();
  float x = getAccelerometerXmg();
  float y = getAccelerometerYmg();
  float z = getAccelerometerZmg();
  return sqrt(x * x + y * y + z * z);
}





float DFRobot_K10Box::getGyroscopeXrads(){
  getQMI8658xyz();
  return (float)(this->gyroX * M_PI)/(this->ssvtG * 180);
}

float DFRobot_K10Box::getGyroscopeYrads(){
  getQMI8658xyz();
  return (float)(this->gyroY * M_PI)/(this->ssvtG * 180);
}

float DFRobot_K10Box::getGyroscopeZrads(){
  getQMI8658xyz();
  return (float)(this->gyroZ * M_PI)/(this->ssvtG * 180);
}


float DFRobot_K10Box::getGyroscopeXdps(){
  getQMI8658xyz();
  return (float)(this->gyroX * 1.0)/(this->ssvtG );
}

float DFRobot_K10Box::getGyroscopeYdps(){
  getQMI8658xyz();
  return (float)(this->gyroY * 1.0)/(this->ssvtG );
}

float DFRobot_K10Box::getGyroscopeZdps(){
  getQMI8658xyz();
  return (float)(this->gyroZ * 1.0)/(this->ssvtG);
}

void DFRobot_K10Box::getQMI8658xyz(){
  uint8_t status,temp[12];
  readReg(QMI8658C_ADDR, 0x2E, &status, 1);
  if(status & 0x03){
    readReg(QMI8658C_ADDR, 0x35, temp, 12);
    this->accX = temp[1]<< 8 | temp[0];
    this->accY = temp[3]<< 8 | temp[2];
    this->accZ = temp[5]<< 8 | temp[4];
    this->gyroX = temp[7]<< 8 | temp[6];
    this->gyroY = temp[9]<< 8 | temp[8];
    this->gyroZ = temp[11]<< 8 | temp[10];
  }
}




void DFRobot_K10Box::writeReg(uint8_t reg, void* pBuf, size_t size){
  uint8_t * _pBuf = (uint8_t *)pBuf;
  Wire.beginTransmission(IO_Device_addr);
  Wire.write(&reg, 1);

  for(uint16_t i = 0; i < size; i++){
    Wire.write(_pBuf[i]);
  }
  Wire.endTransmission();
}

uint8_t DFRobot_K10Box::readReg(uint8_t reg, void* pBuf, size_t size)
{
  uint8_t * _pBuf = (uint8_t *)pBuf;
  Wire.beginTransmission(IO_Device_addr);
  Wire.write(&reg, 1);
  if( Wire.endTransmission() != 0){
      return 0;
  }
  Wire.requestFrom(IO_Device_addr, (uint8_t) size);
  for(uint16_t i = 0; i < size; i++){
    _pBuf[i] = Wire.read();
  }
  return size;
}

void DFRobot_K10Box::writeReg(uint8_t addr, uint8_t reg, void* pBuf, size_t size){
  uint8_t * _pBuf = (uint8_t *)pBuf;
  Wire.beginTransmission(addr);
  Wire.write(&reg, 1);

  for(uint16_t i = 0; i < size; i++){
    Wire.write(_pBuf[i]);
  }
  Wire.endTransmission();
}

uint8_t DFRobot_K10Box::readReg(uint8_t addr, uint8_t reg, void* pBuf, size_t size)
{
  uint8_t * _pBuf = (uint8_t *)pBuf;
  Wire.beginTransmission(addr);
  Wire.write(&reg, 1);
  if( Wire.endTransmission() != 0){
      return 0;
  }
  Wire.requestFrom(addr, (uint8_t) size);
  for(uint16_t i = 0; i < size; i++){
    _pBuf[i] = Wire.read();
  }
  return size;
}

